using System;
using BepuUtilities;
using System.Runtime.CompilerServices;
using BepuUtilities.Memory;
using BepuPhysics.CollisionDetection;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Collidables
{
    /// <summary>
    /// 表示圆柱体的碰撞形状。
    /// </summary>
    public struct Cylinder : IConvexShape
    {
        /// <summary>
        /// 圆柱体的半径。
        /// </summary>
        public float Radius;
        /// <summary>
        /// 圆柱体沿其局部Y轴的一半长度。
        /// </summary>
        public float HalfLength;

        /// <summary>
        /// 获取或设置圆柱体沿其局部Y轴的长度。
        /// </summary>
        public float Length { get { return HalfLength * 2; } set { HalfLength = value * 0.5f; } }

        /// <summary>
        /// 创建圆柱体形状。
        /// </summary>
        /// <param name="Radius">圆柱体的半径。</param>
        /// <param name="length">圆柱体沿其局部Y轴的长度。</param>
        public Cylinder(float radius, float length)
        {
            Radius = radius;
            HalfLength = length * 0.5f;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeAngularExpansionData(out float maximumRadius, out float maximumAngularExpansion)
        {
            maximumRadius = (float)Math.Sqrt(HalfLength * HalfLength + Radius * Radius);
            maximumAngularExpansion = maximumRadius - Math.Min(HalfLength, Radius);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max)
        {
            // 边界框由轴线段和盘帽的贡献组成。
            // 通过对三个方向的极值点进行采样,可以找到盘盖的包围盒：
            // ExtremePointDirection=sampleDirection-点(sampleDirection,轴)*轴
            // ExtremePoint=ExtremePointDirection*Radius/ExtremePointDirection.Length()
            // offsetAlongSampleDirection=点(ExtremePoint,sampleDirection)
            // 这简化了：
            // offsetAlongSampleDirection=dot(ExtremePointDirection*Radius/ExtremePointDirection.Length(),sampleDirection)
            // offsetAlongSampleDirection=dot((sampleDirection-dot(sampleDirection,axis)*axis)*Radius/ExtremePointDirection.Length(),sampleDirection)
            // dot(sampleDirection,sampleDirection-点(sampleDirection,轴)*轴)=1-点(sampleDirection,轴)^2
            // offsetAlongSampleDirection=(1点(sampleDirection,轴)^2)*Radius/ExtremePointDirection.Length()
            // extremePointDirection.Length()=sqrt(dot(sampleDirection-dot(sampleDirection,Axis)*Axis,sampleDirection-dot(sampleDirection,Axis)*Axis)
            // ExtremePointDirection.Length()=sqrt(1-点(sampleDirection,轴)^2-点(sampleDirection,轴)^2+点(sampleDirection,轴)^2*1
            // ExtremePointDirection.Length()=sqrt(1点(sampleDirection,轴)^2
            // OffsetAlongSampleDirection=(1点(sampleDirection,轴)^2)*半径/sqrt(1点(sampleDirection,轴)^2)
            // OffsetAlongSampleDirection=sqrt(1点(sampleDirection,轴)^2)*半径

            // 我们要计算3个不同的样本方向：(1,0,0)、(0,1,0)和(0,0,1)。这等效于简单地访问轴外的组件X、Y或Z。
            // 利用这一事实,我们可以一起计算所有三个方向：
            QuaternionEx.TransformUnitY(orientation, out var y);
            var positiveDiscBoundOffsets = Vector3.SquareRoot(Vector3.Max(Vector3.Zero, Vector3.One - y * y)) * Radius;
            max = Vector3.Abs(HalfLength * y) + positiveDiscBoundOffsets;
            // 圆柱体是对称的。
            min = -max;
        }


        public bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal)
        {
            // 在局部空间工作很方便,因此将光线拉入圆柱体的局部空间。
            Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation);
            var o = origin - pose.Position;
            Matrix3x3.TransformTranspose(o, orientation, out o);
            Matrix3x3.TransformTranspose(direction, orientation, out var d);

            // 规格化方向。Sqrt并没有"那么"糟糕,而且它既简化了事情,又有助于避免数值问题。
            var inverseDLength = 1f / d.Length();
            d *= inverseDLength;

            // 将原点上移到尽可能早的撞击时间。由于数学原因,这并不是必需的,但它确实有助于避免一些数值问题。
            var tOffset = -Vector3.Dot(o, d) - (HalfLength + Radius);
            if (tOffset < 0)
                tOffset = 0;
            o += d * tOffset;
            var oh = new Vector3(o.X, 0, o.Z);
            var dh = new Vector3(d.X, 0, d.Z);
            var a = Vector3.Dot(dh, dh);
            var b = Vector3.Dot(oh, dh);
            var radiusSquared = Radius * Radius;
            var c = Vector3.Dot(oh, oh) - radiusSquared;
            if (b > 0 && c > 0)
            {
                // 雷在外面指着别处,没有击中。
                t = 0;
                normal = new Vector3();
                return false;
            }

            float discY;
            if (a > 1e-8f)
            {
                var discriminant = b * b - a * c;
                if (discriminant < 0)
                {
                    // 无限长的圆柱体不会被击中,所以有限长的圆柱体不会被击中。
                    t = 0;
                    normal = new Vector3();
                    return false;
                }
                t = (-b - (float)Math.Sqrt(discriminant)) / a;
                if (t < -tOffset)
                    t = -tOffset;
                var cylinderHitLocation = o + d * t;
                if (cylinderHitLocation.Y < -HalfLength)
                {
                    discY = -HalfLength;
                }
                else if (cylinderHitLocation.Y > HalfLength)
                {
                    discY = HalfLength;
                }
                else
                {
                    // 打中的是气缸的一侧。
                    normal = new Vector3(cylinderHitLocation.X, 0, cylinderHitLocation.Z) / Radius;
                    Matrix3x3.Transform(normal, orientation, out normal);
                    t = (t + tOffset) * inverseDLength;
                    return true;
                }
            }
            else
            {
                // 射线与轴平行;影响要么落在圆盘上,要么什么都没有。
                discY = d.Y > 0 ? -HalfLength : HalfLength;
            }

            // 将光线与锚定在圆盘Y处的平面相交,法线等于(0,1,0)。
            // t=点(rayOrigin-(0,圆盘Y,0),(0,1,0))/点(rayDirection,(0,1,0))
            if (o.Y * d.Y >= 0)
            {
                // 只有当方向指向圆柱体时,射线才能击中圆盘。
                t = 0;
                normal = new Vector3();
                return false;
            }
            t = (discY - o.Y) / d.Y;
            var hitLocation = o + d * t;
            if (hitLocation.X * hitLocation.X + hitLocation.Z * hitLocation.Z > radiusSquared)
            {
                // 击球未击中球帽。
                t = 0;
                normal = new Vector3();
                return false;
            }
            t = (t + tOffset) * inverseDLength;
            normal = d.Y < 0 ? orientation.Y : -orientation.Y;
            return true;
        }

        public void ComputeInertia(float mass, out BodyInertia inertia)
        {
            inertia.InverseMass = 1f / mass;
            float diagValue = inertia.InverseMass / ((4 * .0833333333f) * HalfLength * HalfLength + .25f * Radius * Radius);
            inertia.InverseInertiaTensor.XX = diagValue;
            inertia.InverseInertiaTensor.YX = 0;
            inertia.InverseInertiaTensor.YY = 2f * inertia.InverseMass / (Radius * Radius);
            inertia.InverseInertiaTensor.ZX = 0;
            inertia.InverseInertiaTensor.ZY = 0;
            inertia.InverseInertiaTensor.ZZ = diagValue;
        }

        public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
        {
            return new ConvexShapeBatch<Cylinder, CylinderWide>(pool, initialCapacity);
        }

        /// <summary>
        /// 圆柱体形状的类型ID。
        /// </summary>
        public const int Id = 4;
        public int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } }
    }

    public struct CylinderWide : IShapeWide<Cylinder>
    {
        public Vector<float> Radius;
        public Vector<float> HalfLength;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Broadcast(in Cylinder shape)
        {
            Radius = new Vector<float>(shape.Radius);
            HalfLength = new Vector<float>(shape.HalfLength);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WriteFirst(in Cylinder source)
        {
            Unsafe.As<Vector<float>, float>(ref Radius) = source.Radius;
            Unsafe.As<Vector<float>, float>(ref HalfLength) = source.HalfLength;
        }

        public bool AllowOffsetMemoryAccess => true;
        public int InternalAllocationSize => 0;
        public void Initialize(in RawBuffer memory) { }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WriteSlot(int index, in Cylinder source)
        {
            GatherScatter.GetOffsetInstance(ref this, index).WriteFirst(source);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector<float> maximumRadius, out Vector<float> maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max)
        {
            QuaternionWide.TransformUnitY(orientations, out var y);
            Vector3Wide.Multiply(y, y, out var yy);
            Vector3Wide.Subtract(Vector<float>.One, yy, out var squared);
            max.X = Vector.Abs(HalfLength * y.X) + Vector.SquareRoot(Vector.Max(Vector<float>.Zero, squared.X)) * Radius;
            max.Y = Vector.Abs(HalfLength * y.Y) + Vector.SquareRoot(Vector.Max(Vector<float>.Zero, squared.Y)) * Radius;
            max.Z = Vector.Abs(HalfLength * y.Z) + Vector.SquareRoot(Vector.Max(Vector<float>.Zero, squared.Z)) * Radius;
            // 圆柱体是对称的。
            Vector3Wide.Negate(max, out min);

            maximumRadius = Vector.SquareRoot(HalfLength * HalfLength + Radius * Radius);
            maximumAngularExpansion = maximumRadius - Vector.Min(HalfLength, Radius);
        }

        public int MinimumWideRayCount
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return 2;
            }
        }

        public void RayTest(ref RigidPoses pose, ref RayWide ray, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
        {
            // 在局部空间工作很方便,因此将光线拉入太空舱的局部空间。
            Matrix3x3Wide.CreateFromQuaternion(pose.Orientation, out var orientation);
            Vector3Wide.Subtract(ray.Origin, pose.Position, out var oWorld);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(oWorld, orientation, out var o);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(ray.Direction, orientation, out var d);

            // 规格化方向。Sqrt并没有"那么"糟糕,而且它既简化了事情,又有助于避免数值问题。
            Vector3Wide.Length(d, out var dLength);
            var inverseDLength = Vector<float>.One / dLength;
            Vector3Wide.Scale(d, inverseDLength, out d);

            // 将原点上移到尽可能早的撞击时间。由于数学原因,这并不是必需的,但它确实有助于避免一些数值问题。
            Vector3Wide.Dot(o, d, out var od);
            var tOffset = Vector.Max(-od - (HalfLength + Radius), Vector<float>.Zero);
            Vector3Wide.Scale(d, tOffset, out var oOffset);
            Vector3Wide.Add(o, oOffset, out o);
            var a = d.X * d.X + d.Z * d.Z;
            var b = o.X * d.X + o.Z * d.Z;
            var radiusSquared = Radius * Radius;
            var c = (o.X * o.X + o.Z * o.Z) - radiusSquared;

            var rayIsntParallel = Vector.GreaterThan(a, new Vector<float>(1e-8f));
            var discriminant = b * b - a * c;
            var cylinderIntersected = Vector.BitwiseAnd(
                Vector.BitwiseOr(
                    Vector.LessThanOrEqual(b, Vector<float>.Zero),
                    Vector.LessThanOrEqual(c, Vector<float>.Zero)),
                Vector.GreaterThanOrEqual(discriminant, Vector<float>.Zero));
            var cylinderT = Vector.Max(-tOffset, (-b - Vector.SquareRoot(discriminant)) / a);
            Vector3Wide.Scale(d, cylinderT, out oOffset);
            Vector3Wide.Add(o, oOffset, out var cylinderHitLocation);
            var inverseRadius = Vector<float>.One / Radius;
            var cylinderNormalX = cylinderHitLocation.X * inverseRadius;
            var cylinderNormalZ = cylinderHitLocation.Z * inverseRadius;
            var useCylinder = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(cylinderHitLocation.Y, -HalfLength), Vector.LessThanOrEqual(cylinderHitLocation.Y, HalfLength));

            // 将最终不使用圆柱体的任何车道的盘盖相交。
            Vector<float> discY = Vector.ConditionalSelect(
                Vector.BitwiseOr(
                    Vector.BitwiseAnd(Vector.GreaterThan(cylinderHitLocation.Y, HalfLength), rayIsntParallel),
                    Vector.AndNot(Vector.LessThanOrEqual(d.Y, Vector<float>.Zero), rayIsntParallel)), HalfLength, -HalfLength);

            // 将光线与锚定在圆盘Y处的平面相交,法线等于(0,1,0)。
            // t=点(rayOrigin-(0,圆盘Y,0),(0,1,0))/点(rayDirection,(0,1,0))
            // 只有当方向指向圆柱体时,射线才能击中圆盘。
            var rayPointsTowardDisc = Vector.LessThan(o.Y * d.Y, Vector<float>.Zero);

            var capT = (discY - o.Y) / d.Y;

            var hitLocationX = o.X + d.X * capT;
            var hitLocationZ = o.Z + d.Z * capT;
            var capHitWithinRadius = Vector.LessThanOrEqual(hitLocationX * hitLocationX + hitLocationZ * hitLocationZ, radiusSquared);
            var hitCap = Vector.BitwiseAnd(rayPointsTowardDisc, capHitWithinRadius);

            t = (tOffset + Vector.ConditionalSelect(useCylinder, cylinderT, Vector.ConditionalSelect(hitCap, capT, Vector<float>.Zero))) * inverseDLength;
            var capUsesUpwardFacingNormal = Vector.LessThan(d.Y, Vector<float>.Zero);
            Vector3Wide localNormal;
            localNormal.X = Vector.ConditionalSelect(useCylinder, cylinderNormalX, Vector<float>.Zero);
            localNormal.Y = Vector.ConditionalSelect(useCylinder, Vector<float>.Zero, Vector.ConditionalSelect(capUsesUpwardFacingNormal, Vector<float>.One, new Vector<float>(-1)));
            localNormal.Z = Vector.ConditionalSelect(useCylinder, cylinderNormalZ, Vector<float>.Zero);
            Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientation, out normal);
            intersected = Vector.ConditionalSelect(useCylinder, cylinderIntersected, hitCap);
        }
    }

    public struct CylinderSupportFinder : ISupportFinder<Cylinder, CylinderWide>
    {
        public bool HasMargin
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return false; }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetMargin(in CylinderWide shape, out Vector<float> margin)
        {
            margin = default;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeSupport(in CylinderWide shape, in Matrix3x3Wide orientation, in Vector3Wide direction, in Vector<int> terminatedLanes, out Vector3Wide support)
        {
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(direction, orientation, out var localDirection);
            ComputeLocalSupport(shape, localDirection, terminatedLanes, out var localSupport);
            Matrix3x3Wide.TransformWithoutOverlap(localSupport, orientation, out support);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeLocalSupport(in CylinderWide shape, in Vector3Wide direction, in Vector<int> terminatedLanes, out Vector3Wide support)
        {
            support.Y = Vector.ConditionalSelect(Vector.GreaterThan(direction.Y, Vector<float>.Zero), shape.HalfLength, -shape.HalfLength);
            // TODO：在这里使用硬件加速的倒数SQRT近似将非常有益。
            // 在依赖圆柱体的模拟中使用全帧时间实际上是有意义的。
            var horizontalLength = Vector.SquareRoot(direction.X * direction.X + direction.Z * direction.Z);
            var normalizeScale = shape.Radius / horizontalLength;
            var useHorizontal = Vector.GreaterThan(horizontalLength, new Vector<float>(1e-8f));
            support.X = Vector.ConditionalSelect(useHorizontal, direction.X * normalizeScale, Vector<float>.Zero);
            support.Z = Vector.ConditionalSelect(useHorizontal, direction.Z * normalizeScale, Vector<float>.Zero);
        }
    }
}
